// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: compute_sideslip_h_and_k
 *
 * Args:
 *     state: Matrix24_1
 *     P: Matrix24_24
 *     innov_var: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     H: Matrix24_1
 *     K: Matrix24_1
 */
template <typename Scalar>
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
  // Total ops: 533

  // Input arrays

  // Intermediate terms (40)
  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
  const Scalar _tmp1 = 2 * state(3, 0);
  const Scalar _tmp2 = 2 * state(6, 0);
  const Scalar _tmp3 = _tmp2 * state(2, 0);
  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
  const Scalar _tmp6 = -state(22, 0) + state(4, 0);
  const Scalar _tmp7 = _tmp1 * state(0, 0);
  const Scalar _tmp8 = 2 * state(1, 0) * state(2, 0);
  const Scalar _tmp9 = _tmp7 + _tmp8;
  const Scalar _tmp10 = 2 * state(0, 0);
  const Scalar _tmp11 = _tmp1 * state(1, 0) - _tmp10 * state(2, 0);
  const Scalar _tmp12 = _tmp0 * _tmp9 + _tmp11 * state(6, 0) + _tmp5 * _tmp6;
  const Scalar _tmp13 =
      _tmp12 + epsilon * (2 * math::min<Scalar>(0, (((_tmp12) > 0) - ((_tmp12) < 0))) + 1);
  const Scalar _tmp14 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
  const Scalar _tmp15 = -_tmp7 + _tmp8;
  const Scalar _tmp16 = _tmp1 * state(2, 0) + _tmp10 * state(1, 0);
  const Scalar _tmp17 =
      (_tmp0 * _tmp14 + _tmp15 * _tmp6 + _tmp16 * state(6, 0)) / std::pow(_tmp13, Scalar(2));
  const Scalar _tmp18 = _tmp2 * state(1, 0);
  const Scalar _tmp19 = Scalar(1.0) / (_tmp13);
  const Scalar _tmp20 = -_tmp17 * (_tmp0 * _tmp1 - _tmp3) + _tmp19 * (-_tmp1 * _tmp6 + _tmp18);
  const Scalar _tmp21 = 2 * _tmp0;
  const Scalar _tmp22 = _tmp1 * state(6, 0);
  const Scalar _tmp23 = 4 * _tmp0;
  const Scalar _tmp24 = 2 * _tmp6;
  const Scalar _tmp25 = _tmp2 * state(0, 0);
  const Scalar _tmp26 = -_tmp17 * (_tmp21 * state(2, 0) + _tmp22) +
                        _tmp19 * (-_tmp23 * state(1, 0) + _tmp24 * state(2, 0) + _tmp25);
  const Scalar _tmp27 = 4 * _tmp6;
  const Scalar _tmp28 = -_tmp17 * (_tmp21 * state(1, 0) - _tmp25 - _tmp27 * state(2, 0)) +
                        _tmp19 * (_tmp22 + _tmp24 * state(1, 0));
  const Scalar _tmp29 = -_tmp17 * (_tmp18 + _tmp21 * state(0, 0) - _tmp27 * state(3, 0)) +
                        _tmp19 * (-_tmp23 * state(3, 0) - _tmp24 * state(0, 0) + _tmp3);
  const Scalar _tmp30 = _tmp17 * _tmp5;
  const Scalar _tmp31 = _tmp15 * _tmp19;
  const Scalar _tmp32 = -_tmp30 + _tmp31;
  const Scalar _tmp33 = _tmp17 * _tmp9;
  const Scalar _tmp34 = _tmp14 * _tmp19;
  const Scalar _tmp35 = -_tmp33 + _tmp34;
  const Scalar _tmp36 = -_tmp11 * _tmp17 + _tmp16 * _tmp19;
  const Scalar _tmp37 = _tmp30 - _tmp31;
  const Scalar _tmp38 = _tmp33 - _tmp34;
  const Scalar _tmp39 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));

  // Output terms (2)
  if (H != nullptr) {
    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

    _h(0, 0) = _tmp20;
    _h(1, 0) = _tmp26;
    _h(2, 0) = _tmp28;
    _h(3, 0) = _tmp29;
    _h(4, 0) = _tmp32;
    _h(5, 0) = _tmp35;
    _h(6, 0) = _tmp36;
    _h(22, 0) = _tmp37;
    _h(23, 0) = _tmp38;
  }

  if (K != nullptr) {
    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

    _k(0, 0) = _tmp39 * (P(0, 0) * _tmp20 + P(0, 1) * _tmp26 + P(0, 2) * _tmp28 +
                         P(0, 22) * _tmp37 + P(0, 23) * _tmp38 + P(0, 3) * _tmp29 +
                         P(0, 4) * _tmp32 + P(0, 5) * _tmp35 + P(0, 6) * _tmp36);
    _k(1, 0) = _tmp39 * (P(1, 0) * _tmp20 + P(1, 1) * _tmp26 + P(1, 2) * _tmp28 +
                         P(1, 22) * _tmp37 + P(1, 23) * _tmp38 + P(1, 3) * _tmp29 +
                         P(1, 4) * _tmp32 + P(1, 5) * _tmp35 + P(1, 6) * _tmp36);
    _k(2, 0) = _tmp39 * (P(2, 0) * _tmp20 + P(2, 1) * _tmp26 + P(2, 2) * _tmp28 +
                         P(2, 22) * _tmp37 + P(2, 23) * _tmp38 + P(2, 3) * _tmp29 +
                         P(2, 4) * _tmp32 + P(2, 5) * _tmp35 + P(2, 6) * _tmp36);
    _k(3, 0) = _tmp39 * (P(3, 0) * _tmp20 + P(3, 1) * _tmp26 + P(3, 2) * _tmp28 +
                         P(3, 22) * _tmp37 + P(3, 23) * _tmp38 + P(3, 3) * _tmp29 +
                         P(3, 4) * _tmp32 + P(3, 5) * _tmp35 + P(3, 6) * _tmp36);
    _k(4, 0) = _tmp39 * (P(4, 0) * _tmp20 + P(4, 1) * _tmp26 + P(4, 2) * _tmp28 +
                         P(4, 22) * _tmp37 + P(4, 23) * _tmp38 + P(4, 3) * _tmp29 +
                         P(4, 4) * _tmp32 + P(4, 5) * _tmp35 + P(4, 6) * _tmp36);
    _k(5, 0) = _tmp39 * (P(5, 0) * _tmp20 + P(5, 1) * _tmp26 + P(5, 2) * _tmp28 +
                         P(5, 22) * _tmp37 + P(5, 23) * _tmp38 + P(5, 3) * _tmp29 +
                         P(5, 4) * _tmp32 + P(5, 5) * _tmp35 + P(5, 6) * _tmp36);
    _k(6, 0) = _tmp39 * (P(6, 0) * _tmp20 + P(6, 1) * _tmp26 + P(6, 2) * _tmp28 +
                         P(6, 22) * _tmp37 + P(6, 23) * _tmp38 + P(6, 3) * _tmp29 +
                         P(6, 4) * _tmp32 + P(6, 5) * _tmp35 + P(6, 6) * _tmp36);
    _k(7, 0) = _tmp39 * (P(7, 0) * _tmp20 + P(7, 1) * _tmp26 + P(7, 2) * _tmp28 +
                         P(7, 22) * _tmp37 + P(7, 23) * _tmp38 + P(7, 3) * _tmp29 +
                         P(7, 4) * _tmp32 + P(7, 5) * _tmp35 + P(7, 6) * _tmp36);
    _k(8, 0) = _tmp39 * (P(8, 0) * _tmp20 + P(8, 1) * _tmp26 + P(8, 2) * _tmp28 +
                         P(8, 22) * _tmp37 + P(8, 23) * _tmp38 + P(8, 3) * _tmp29 +
                         P(8, 4) * _tmp32 + P(8, 5) * _tmp35 + P(8, 6) * _tmp36);
    _k(9, 0) = _tmp39 * (P(9, 0) * _tmp20 + P(9, 1) * _tmp26 + P(9, 2) * _tmp28 +
                         P(9, 22) * _tmp37 + P(9, 23) * _tmp38 + P(9, 3) * _tmp29 +
                         P(9, 4) * _tmp32 + P(9, 5) * _tmp35 + P(9, 6) * _tmp36);
    _k(10, 0) = _tmp39 * (P(10, 0) * _tmp20 + P(10, 1) * _tmp26 + P(10, 2) * _tmp28 +
                          P(10, 22) * _tmp37 + P(10, 23) * _tmp38 + P(10, 3) * _tmp29 +
                          P(10, 4) * _tmp32 + P(10, 5) * _tmp35 + P(10, 6) * _tmp36);
    _k(11, 0) = _tmp39 * (P(11, 0) * _tmp20 + P(11, 1) * _tmp26 + P(11, 2) * _tmp28 +
                          P(11, 22) * _tmp37 + P(11, 23) * _tmp38 + P(11, 3) * _tmp29 +
                          P(11, 4) * _tmp32 + P(11, 5) * _tmp35 + P(11, 6) * _tmp36);
    _k(12, 0) = _tmp39 * (P(12, 0) * _tmp20 + P(12, 1) * _tmp26 + P(12, 2) * _tmp28 +
                          P(12, 22) * _tmp37 + P(12, 23) * _tmp38 + P(12, 3) * _tmp29 +
                          P(12, 4) * _tmp32 + P(12, 5) * _tmp35 + P(12, 6) * _tmp36);
    _k(13, 0) = _tmp39 * (P(13, 0) * _tmp20 + P(13, 1) * _tmp26 + P(13, 2) * _tmp28 +
                          P(13, 22) * _tmp37 + P(13, 23) * _tmp38 + P(13, 3) * _tmp29 +
                          P(13, 4) * _tmp32 + P(13, 5) * _tmp35 + P(13, 6) * _tmp36);
    _k(14, 0) = _tmp39 * (P(14, 0) * _tmp20 + P(14, 1) * _tmp26 + P(14, 2) * _tmp28 +
                          P(14, 22) * _tmp37 + P(14, 23) * _tmp38 + P(14, 3) * _tmp29 +
                          P(14, 4) * _tmp32 + P(14, 5) * _tmp35 + P(14, 6) * _tmp36);
    _k(15, 0) = _tmp39 * (P(15, 0) * _tmp20 + P(15, 1) * _tmp26 + P(15, 2) * _tmp28 +
                          P(15, 22) * _tmp37 + P(15, 23) * _tmp38 + P(15, 3) * _tmp29 +
                          P(15, 4) * _tmp32 + P(15, 5) * _tmp35 + P(15, 6) * _tmp36);
    _k(16, 0) = _tmp39 * (P(16, 0) * _tmp20 + P(16, 1) * _tmp26 + P(16, 2) * _tmp28 +
                          P(16, 22) * _tmp37 + P(16, 23) * _tmp38 + P(16, 3) * _tmp29 +
                          P(16, 4) * _tmp32 + P(16, 5) * _tmp35 + P(16, 6) * _tmp36);
    _k(17, 0) = _tmp39 * (P(17, 0) * _tmp20 + P(17, 1) * _tmp26 + P(17, 2) * _tmp28 +
                          P(17, 22) * _tmp37 + P(17, 23) * _tmp38 + P(17, 3) * _tmp29 +
                          P(17, 4) * _tmp32 + P(17, 5) * _tmp35 + P(17, 6) * _tmp36);
    _k(18, 0) = _tmp39 * (P(18, 0) * _tmp20 + P(18, 1) * _tmp26 + P(18, 2) * _tmp28 +
                          P(18, 22) * _tmp37 + P(18, 23) * _tmp38 + P(18, 3) * _tmp29 +
                          P(18, 4) * _tmp32 + P(18, 5) * _tmp35 + P(18, 6) * _tmp36);
    _k(19, 0) = _tmp39 * (P(19, 0) * _tmp20 + P(19, 1) * _tmp26 + P(19, 2) * _tmp28 +
                          P(19, 22) * _tmp37 + P(19, 23) * _tmp38 + P(19, 3) * _tmp29 +
                          P(19, 4) * _tmp32 + P(19, 5) * _tmp35 + P(19, 6) * _tmp36);
    _k(20, 0) = _tmp39 * (P(20, 0) * _tmp20 + P(20, 1) * _tmp26 + P(20, 2) * _tmp28 +
                          P(20, 22) * _tmp37 + P(20, 23) * _tmp38 + P(20, 3) * _tmp29 +
                          P(20, 4) * _tmp32 + P(20, 5) * _tmp35 + P(20, 6) * _tmp36);
    _k(21, 0) = _tmp39 * (P(21, 0) * _tmp20 + P(21, 1) * _tmp26 + P(21, 2) * _tmp28 +
                          P(21, 22) * _tmp37 + P(21, 23) * _tmp38 + P(21, 3) * _tmp29 +
                          P(21, 4) * _tmp32 + P(21, 5) * _tmp35 + P(21, 6) * _tmp36);
    _k(22, 0) = _tmp39 * (P(22, 0) * _tmp20 + P(22, 1) * _tmp26 + P(22, 2) * _tmp28 +
                          P(22, 22) * _tmp37 + P(22, 23) * _tmp38 + P(22, 3) * _tmp29 +
                          P(22, 4) * _tmp32 + P(22, 5) * _tmp35 + P(22, 6) * _tmp36);
    _k(23, 0) = _tmp39 * (P(23, 0) * _tmp20 + P(23, 1) * _tmp26 + P(23, 2) * _tmp28 +
                          P(23, 22) * _tmp37 + P(23, 23) * _tmp38 + P(23, 3) * _tmp29 +
                          P(23, 4) * _tmp32 + P(23, 5) * _tmp35 + P(23, 6) * _tmp36);
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
